#ifndef  CLMOTOR_H
#define CLMOTOR_H
#include "encoder.h"
#include "speed_sensor.h"
#include "pid.h"
#include "motor.h"
typedef struct {
	PIDController pid;    //PID控制器
    SpeedSensor speed;    //电机测速,内含编码器
    Motor motor;          //电机驱动
    uint8_t enabled;      //
	uint8_t update_counter;
} ClosedLoopMotor;

void clmotor_init(ClosedLoopMotor *m,
                  GPIO_PinName enc_a, 
                  GPIO_PinName enc_b,
                  uint16_t ppr,
                  uint32_t interval_ms,  // 0=MT模式，>0=M模式（ms）
float kp, float ki, float kd,  float dt,//若为mt法，取低速定时器的dt
                  GPIO_PinName pwm_pin, 
                  GPIO_PinName dir_pin1, GPIO_PinName dir_pin2);

void clmotor_update(ClosedLoopMotor *m);
void clmotor_set_target(ClosedLoopMotor *m, float rpm);

inline void clmotor_set_target(ClosedLoopMotor *m, float rpm) {
	m->pid.target = rpm;
}

inline float clmotor_get_now_rpm(ClosedLoopMotor *m) {
	return speed_get_rpm(&m->speed);
}

inline float clmotor_get_output(ClosedLoopMotor *m) {
	return m->pid.output;
}

static inline void clmotor_enable(ClosedLoopMotor *m) {
	m->pid.integral = 0; //启动时清积分
    m->enabled = 1;
}

static inline void clmotor_disable(ClosedLoopMotor *m) {
    m->enabled = 0;
    motor_set(&m->motor,0, 0);  // 关闭电机
}

#endif
